from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, TimerAction, RegisterEventHandler
from launch.event_handlers import OnProcessStart
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
import os

def generate_launch_description():
    # 获取包路径
    go2_desc_pkg = get_package_share_directory("go2_description")
    go2_driver_pkg = get_package_share_directory("go2_driver_py")
    lslidar_trans_pkg = get_package_share_directory("go2_perception")
    go2_gmapping_pkg = get_package_share_directory("slam_gmapping")

    # # 1. 启动雷达驱动
    lslidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(lslidar_trans_pkg, "launch", "go2_pointcloud_process.launch.py")
        )
    )

    # 2. 启动 URDF（TF 树）
    display_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(go2_desc_pkg, "launch", "display.launch.py")
        ),
        launch_arguments={"use_joint_state_publisher": "false"}.items()
    )

    # 3. 启动 go2_driver（发布里程计）
    go2_driver_node = Node(
        package="go2_driver_py",
        executable="driver",
        parameters=[os.path.join(go2_driver_pkg, "params", "go2_driver.yaml")],
        output="screen",
        emulate_tty=True,
    )

    # 4. 延迟启动 gmapping（确保 TF 和雷达数据就绪）
    gmapping_node = Node(
        package="slam_gmapping",
        executable="slam_gmapping",
        output="screen",
        parameters=[os.path.join(go2_gmapping_pkg, "params", "slam_gmapping.yaml")],
    )

    # 5. 延迟启动 rviz2（可视化）
    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        arguments=["-d", os.path.join(go2_gmapping_pkg, "rviz", "go2.rviz")],
        output="screen",
    )

    # 使用 TimerAction 确保关键节点先启动
    delayed_gmapping = TimerAction(
        period=3.0,  # 延迟 3 秒
        actions=[gmapping_node],
    )

    delayed_rviz = TimerAction(
        period=5.0,  # 延迟 5 秒（确保 gmapping 已经初始化）
        actions=[rviz2_node],
    )

    return LaunchDescription([
        # 先启动雷达、URDF 和 driver
        lslidar_launch,
        display_launch,
        go2_driver_node,

        # 延迟启动 gmapping 和 rviz
        delayed_gmapping,
        delayed_rviz,
    ])